//
// Created by fazhehy on 2024/7/24.
//

#include "encoder.h"

#define RISING  0
#define FALLING 1

int16_t left_counter = 0;
int16_t right_counter = 0;

int16_t left_speed = 0;
int32_t left_distance = 0;
int16_t right_speed = 0;
int32_t right_distance = 0;

void encoder_init()
{
    NVIC_EnableIRQ(GPIOA_INT_IRQn);
    NVIC_EnableIRQ(GPIOB_INT_IRQn);
}

void encoder_callback()
{
//    left_speed = (int16_t)(0.7f*left_speed + 0.3f*left_counter);
    left_speed = left_counter;
    left_counter = 0;
    left_distance += left_speed;
//    right_speed = (int16_t)(0.7f*right_speed + 0.3f*right_counter);
    right_speed = right_counter;
    right_counter = 0;
    right_distance += right_speed;
}

void encoder_exti_callback(void)
{
    uint32_t gpioA = DL_GPIO_getEnabledInterruptStatus(GPIOA, DL_GPIO_PIN_25 | DL_GPIO_PIN_26);
    uint32_t gpioB = DL_GPIO_getEnabledInterruptStatus(GPIOB, DL_GPIO_PIN_8 | DL_GPIO_PIN_9);
    if ((gpioA & DL_GPIO_PIN_25) == DL_GPIO_PIN_25){
        DL_GPIO_clearInterruptStatus(GPIOA, DL_GPIO_PIN_25);
        if (DL_GPIO_readPins(GPIOA, DL_GPIO_PIN_25) != 0){
            if (DL_GPIO_readPins(GPIOA, DL_GPIO_PIN_26) != 0)
                right_counter --;
            else
                right_counter ++;
        }
        else{
            if (DL_GPIO_readPins(GPIOA, DL_GPIO_PIN_26) != 0)
                right_counter ++;
            else
                right_counter --;
        }
    }
    if ((gpioA & DL_GPIO_PIN_26) == DL_GPIO_PIN_26){
        DL_GPIO_clearInterruptStatus(GPIOA, DL_GPIO_PIN_26);
        if (DL_GPIO_readPins(GPIOA, DL_GPIO_PIN_26) != 0){
            if (DL_GPIO_readPins(GPIOA, DL_GPIO_PIN_25) != 0)
                right_counter ++;
            else
                right_counter --;
        }
        else{
            if (DL_GPIO_readPins(GPIOA, DL_GPIO_PIN_25) != 0)
                right_counter --;
            else
                right_counter ++;
        }
    }
    if ((gpioB & DL_GPIO_PIN_8) == DL_GPIO_PIN_8){
        DL_GPIO_clearInterruptStatus(GPIOB, DL_GPIO_PIN_8);
        if (DL_GPIO_readPins(GPIOB, DL_GPIO_PIN_8) != 0){
            if (DL_GPIO_readPins(GPIOB, DL_GPIO_PIN_9) != 0)
                left_counter --;
            else
                left_counter ++;
        }
        else{
            if (DL_GPIO_readPins(GPIOB, DL_GPIO_PIN_9) != 0)
                left_counter ++;
            else
                left_counter --;
        }
    }
    if ((gpioB & DL_GPIO_PIN_9) == DL_GPIO_PIN_9){
        DL_GPIO_clearInterruptStatus(GPIOB, DL_GPIO_PIN_9);
        if (DL_GPIO_readPins(GPIOB, DL_GPIO_PIN_9) != 0){
            if (DL_GPIO_readPins(GPIOB, DL_GPIO_PIN_8) != 0)
                left_counter ++;
            else
                left_counter --;
        }
        else{
            if (DL_GPIO_readPins(GPIOB, DL_GPIO_PIN_8) != 0)
                left_counter --;
            else
                left_counter ++;
        }
    }
}
